last edits:
This notebook creates a figure that compares predictions of different models (simulation and affine) with experimental data. It relies on the "AnkleSLIP - find minimal model - Version 3"-Notebook
In [1]:
# run this to connect an ipython qtconsole to the notebook
%connect_info
In [1]:
%cd
%cd mmnotebooks
In [2]:
# --- run required cells automatically? :) (notebook must be stored for this!)
import mutils.io as mio
import mutils.misc as mi
# load basic config
nb_name = 'AnkleSLIP - find minimal model- Version 3.ipynb'
mi.run_nbcells(nb_name, ['0'])
conf.subject = 3
conf.quiet = True
mi.run_nbcells(nb_name,
['0.1','1', '3', '3.1', '3.2'])
print "notebook initialized"
In [3]:
# --- compute distance from baseline to SLIP periodic orbit for every stride
if False: # requires simulation models ... run "SLIP predictions" cell first!
f = ws1.dataset_full # shortcut
dpr = fda.dt_movingavg(f.all_param_r, conf.dt_window, conf.dt_medfilter)
dpl = fda.dt_movingavg(f.all_param_l, conf.dt_window, conf.dt_medfilter)
scalesr = std(dpr, axis=0)
pbr = f.all_param_r - dpr # baseline
deltar = (pbr - ws1.cslip.Pp_r)/scalesr
scalesl = std(dpl, axis=0)
pbl = f.all_param_l - dpl # baseline
deltal = (pbl - ws1.cslip.Pp_l)/scalesl
# --- --- repeat block for apex states
dpr = fda.dt_movingavg(f.all_IC_r, conf.dt_window, conf.dt_medfilter)
dpl = fda.dt_movingavg(f.all_IC_l, conf.dt_window, conf.dt_medfilter)
scalesr = std(dpr, axis=0)
pbr = f.all_IC_r - dpr # baseline
deltar_ap = (pbr - ws1.cslip.ICp_r)/scalesr
scalesl = std(dpl, axis=0)
pbl = f.all_IC_l - dpl # baseline
deltal_ap = (pbl - ws1.cslip.ICp_l)/scalesl
figure(figsize=(12,4))
plot(sum(deltar**2, axis=1),'b.', label="right leg params")
plot(sum(deltal**2, axis=1),'r.', label="left leg params")
title('deviation from baseline to periodic SLIP')
plot(-sum(deltar_ap**2, axis=1),'g.', label="right apex states")
plot(-sum(deltal_ap**2, axis=1),'m.', label="left apex states")
plot(array(gca().get_xlim())*.95, [0, 0], 'k--')
gca().set_ylim(-3,3)
legend(loc='best')
# zoom manually
#xlim(900,1000)
grid('on')
In [4]:
# --- init section 10
if not 'ws10' in locals():
ws10 = mio.saveable()
ws10.nps = 50 # sections per stride
ws10.strides = [1400, 1401, 1402, 1403, 1404] # reasonable choices according to test above:
ws10.strides = [1202, 1203, 1204, 1205, 1206] # reasonable choices according to test above:
ws10.strides = [1202, 1203, 1204, 1205] # subject 7
ws10.otheridx = fda.otheridx(ws10.strides, ws1.dataset_full.all_IC_r.shape[0])
# subject 1: [930, 931]
# subject 2: [1500, 1501]
# subject 3: [1203, 1204]
# subject 7: [1490, 1491]
ws10.sct0 = 1 # index of section of 1D-data (which start at apex) to start the base Floquet model from
ws10.tspace = .0 # time in the plots between adjacent strides
In [5]:
# --- build dataset and "base" floquet model (full)
# --- --- prerequisites: compute phases
ws10.phi_r = mod(hstack(ws1.dataset_full.all_phases_r), 2*pi)
ws10.phi_r_doc = "phases of right apices (mod 2pi)"
ws10.phi_l = mod(hstack(ws1.dataset_full.all_phases_l), 2*pi)
ws10.phi_l_doc = "phases of left apices (mod 2pi)"
ws10.phi_r0 = mean(ws10.phi_r)
ws10.phi_r0_doc = "average phase at right apex"
ws10.phi_l0 = mean(ws10.phi_l)
ws10.phi_l0_doc = "average phase at left apex"
# --- --- build dataset
sys.stdout.write('building dataset ...')
ws1.k.selection = ws1.full_markers
ws10.kin_out_r = ws1.k.make_1D(ws10.nps, phases_list = ws1.dataset_full.all_phases_r)[:, ws10.nps*2:]
ws10.kin_out_r_dt = fda.dt_movingavg(ws10.kin_out_r, conf.dt_window,
conf.dt_medfilter)
ws10.kin_out_r_labels = ws1.k.selection[2:] + ['v_' + elem for elem in ws1.k.selection]
ws10.kin_out_r_doc = "1D format of data (right) (without processing, start with com height"
ws10.kin_out_r_dt_doc = "detrended version of kin_out"
#ws10.kin_out_l = ws1.k.make_1D(ws10.nps, phases_list = ws1.dataset_full.all_phases_l)[:, ws10.nps*2:]
#ws10.kin_out_l_dt = fda.dt_movingavg(ws10.kin_out_l, conf.dt_window,
# conf.dt_medfilter)
#ws10.kin_out_l_doc = "1D format of data (left) (without processing, start with com_z"
#ws10.kin_out_l_dt_doc = "detrended version of kin_out"
ws10.kins_full = ws1.k.get_kin() # kins is <2n #makers -by- 60000> array -> select [2:, :]
ws10.kins_full_doc = " <2n #makers -by- 60000> array containing all continous kinematics"
# obtain labels for dimensions
lblv = [x for x in ws1.dataset_full.kin_labels if x.startswith('v_')]
ws10.kin_labels = ([x for x in ws1.dataset_full.kin_labels if not x.startswith('v_')]
+ lblv[:2][:] + ['v_com_z', ] + lblv[2:][:])
#ws10.phi_out_r = linspace(ws10.phi_l0, ws10, phi_l0 + 2*pi, ws10.nps)
nocom_idx = hstack([idx for idx, nm in enumerate(ws1.dataset_full.kin_labels) if '-' in nm])
ws10.idat_full_r = hstack([ws1.dataset_full.all_IC_r,
ws1.dataset_full.all_kin_r[:, nocom_idx]])
ws10.idat_full_r = mi.dt_movingavg(ws10.idat_full_r, conf.dt_window, conf.dt_medfilter)
print "done!"
sys.stdout.write('building CoM output data ...')
ws1.k.selection = ['com_x', 'com_y', 'com_z' ]
#ws1.k.selection = ['com_y', 'com_x', 'com_z' ]
re_ord_idx = hstack( [idx * ws10.nps + arange(ws10.nps) for idx in [2,4,5,3]])
ws10.odat_full = ws1.k.make_1D(ws10.nps, phases_list = ws1.dataset_full.all_phases_r)[:, re_ord_idx]
ws10.odat = fda.dt_movingavg(ws10.odat_full, conf.dt_window, conf.dt_medfilter)
# compute baseline
import mutils.FDatAn as fda
ws10.baseline = fda.oneD_twoD(mean(ws10.odat_full, axis=0)[newaxis,:], ws10.nps).T
ws10.baseline = vstack([ws10.baseline, ws10.baseline[-1:,:]])
ws10.baseline_std = fda.oneD_twoD(std(ws10.odat_full, axis=0)[newaxis,:], ws10.nps).T
ws10.baseline_std = vstack([ws10.baseline_std, ws10.baseline_std[-1:,:]])
print "done!"
In [6]:
#mean(ws1.dataset_full.all_IC_r, axis=0)
figure(), plot(ws10.baseline[:,3
])
#ws10.odat.shape
Out[6]:
In [7]:
#ws10.idat_full_r
#------------ create right affine model
print "building affine models..."
print "right stride: ",
idat = ws10.idat_full_r[ws10.otheridx, :]
fmdls = []
for secnr in range(ws10.sct0, ws10.nps):
odat = ws10.odat[ws10.otheridx, secnr::ws10.nps]
map_ = dot(odat.T, pinv(idat.T, rcond=1e-7))
#_, maps, _ = fda.fitData(idat, odat, nps=1, nrep=50)
fmdls.append(map_)
sys.stdout.write('.')
# append map to section 0 (+2pi)
idat = ws10.idat_full_r[ws10.otheridx, :][:-1,:]
odat = ws10.odat[ws10.otheridx,:][1:, ::ws10.nps]
map_ = dot(odat.T, pinv(idat.T, rcond=1e-7))
fmdls.append(map_)
ws10.fmdls_full_r = fmdls[:]
ws10.fmdls_full_r_doc = ("Affine. models full state -> CoM, subject {},".format(conf.subject))
print "Full state affine model computed!"
In [8]:
# old version: base models + initial mappings (new: direct mappings)
if False:
# --- --- build "base" floquet models
# oidx: selected output dimensions
vidx = len([x for x in ws10.kin_labels if not x.startswith('v_')])
oidx = hstack([arange(ws10.nps), #com_z
arange(ws10.nps*vidx,ws10.nps*(vidx + 3)), #v_com_x ... v_com_z
])
#------------ create right "basic" Floquet model
print "building base floquet models..."
print "right stride: ",
ws10.idat_base_r = ws10.kin_out_r_dt[:, ws10.sct0::ws10.nps] # start from 3rd section! (far enough from apex)
ws10.idat_base_r_doc = "(detrended) data at starting section for right Floquet model"
idat = ws10.idat_base_r.copy()
#phi_ref = ws10.phi_r0 + ws10.sct0 * (2.*pi/ws10.nps)
odat_0 = ws10.kin_out_r_dt[:, oidx] # complete odat; including apex (which is "before" section0 of floquet model)
fmdls = []
for secnr in range(ws10.sct0 + 1, ws10.nps):
odat = odat_0[:, secnr::ws10.nps]
_, maps, _ = fda.fitData(idat, odat, nps=1, nrep=50)
fmdls.append(fda.meanMat(maps))
sys.stdout.write('.')
# append map to section 0 (+2pi)
idat = ws10.kin_out_r_dt[:-1, ws10.sct0::ws10.nps]
odat = odat_0[1:, ::ws10.nps]
_, maps, _ = fda.fitData(idat, odat, nps=1, nrep=50)
fmdls.append(fda.meanMat(maps))
ws10.fmdls_r = fmdls[:]
ws10.fmdls_r_doc = ("Lin. models kinematics -> CoM, subject {}," +
" from sect. r. apex+2 to next apex (incl.)".format(conf.subject))
print "done!"
#------------ create left "basic" Floquet model
print "building base floquet models..."
print "left stride: ",
ws10.idat_base_l = ws10.kin_out_l_dt[:, ws10.sct0::ws10.nps] # start from 3rd section! (far enough from apex)
ws10.idat_base_l_doc = "(detrended) data at starting section for left Floquet model"
idat = ws10.idat_base_l.copy()
#phi_ref = ws10.phi_l0 + ws10.sct0 * (2.*pi/ws10.nps)
odat_0 = ws10.kin_out_l_dt[:, oidx] # complete odat; including apex (which is "before" section0 of floquet model)
fmdls = []
for secnr in range(ws10.sct0 + 1, ws10.nps):
odat = odat_0[:, secnr::ws10.nps]
_, maps, _ = fda.fitData(idat, odat, nps=1, nrep=50)
fmdls.append(fda.meanMat(maps))
sys.stdout.write('.')
# append map to section 0 (+2pi)
idat = ws10.kin_out_l_dt[:-1, ws10.sct0::ws10.nps]
odat = odat_0[1:, ::ws10.nps]
_, maps, _ = fda.fitData(idat, odat, nps=1, nrep=50)
fmdls.append(fda.meanMat(maps))
ws10.fmdls_l = fmdls[:]
ws10.fmdls_l_doc = ("Lin. models kinematics -> CoM, subject {}," +
" from sect. l. apex+2 to next apex (incl.)".format(conf.subject))
print "done!"
In [9]:
# --- build dataset
# --- --- build dataset
sys.stdout.write('building dataset for ankle-SLIP...')
ws1.k.selection = ['com_x', 'com_y', 'com_z', 'r_anl_x - com_x', 'r_anl_y - com_y', 'r_anl_z - com_z']
ws10.kin_out_ank_r = ws1.k.make_1D(ws10.nps, phases_list = ws1.dataset_full.all_phases_r)[:, ws10.nps*2:]
ws10.kin_out_ank_r_dt = fda.dt_movingavg(ws10.kin_out_ank_r, conf.dt_window,
conf.dt_medfilter)
ws10.kin_out_r_doc = "1D format of data (right ankle SLIP) (without processing, start with com_z"
ws10.kin_out_r_dt_doc = "detrended version of kin_out_ank_r"
ws10.kin_out_ank_l = ws1.k.make_1D(ws10.nps, phases_list = ws1.dataset_full.all_phases_l)[:, ws10.nps*2:]
ws10.kin_out_ank_l_dt = fda.dt_movingavg(ws10.kin_out_ank_l, conf.dt_window,
conf.dt_medfilter)
ws10.kin_out_ank_l_doc = "1D format of data (left) (without processing, start with com_z"
ws10.kin_out_ank_l_dt_doc = "detrended version of kin_out"
ws10.kins_ank = ws1.k.get_kin() # kins is <2n #makers -by- 60000> array -> select [2:, :]
ws10.kins_ank_doc = " <2n #makers -by- 60000> array containing ankle-SLIP continous kinematics"
# obtain labels for dimensions
lblv = [x for x in ws1.dataset_full.kin_labels if x.startswith('v_')]
ws10.kin_labels = ([x for x in ws1.dataset_full.kin_labels if not x.startswith('v_')]
+ lblv[:2][:] + ['v_com_z', ] + lblv[2:][:])
ws10.kin_labels_ank = ws1.k.selection[2:] + ['v_' + x for x in ws1.k.selection]
ws10.kin_labels_ank_doc = 'labels for the kinematic state of ankle-SLIP dimensions'
#ws10.phi_out_r = linspace(ws10.phi_l0, ws10, phi_l0 + 2*pi, ws10.nps)
print "done!"
nocom_1Didx = hstack([idx*ws10.nps for idx, nm in enumerate(ws10.kin_labels_ank) if '-' in nm])
ws10.idat_ank_r = hstack([ws1.dataset_full.all_IC_r,
ws10.kin_out_ank_r[:, nocom_1Didx]])
ws10.idat_ank_r = mi.dt_movingavg(ws10.idat_ank_r, conf.dt_window, conf.dt_medfilter)
#------------ create right affine model
print "building affine models..."
print "right stride: ",
idat = ws10.idat_ank_r[ws10.otheridx, :]
fmdls = []
for secnr in range(ws10.sct0, ws10.nps):
odat = ws10.odat[ws10.otheridx, secnr::ws10.nps]
map_ = dot(odat.T, pinv(idat.T, rcond=1e-7))
#_, maps, _ = fda.fitData(idat, odat, nps=1, nrep=50)
fmdls.append(map_)
sys.stdout.write('.')
# append map to section 0 (+2pi)
idat = ws10.idat_ank_r[ws10.otheridx,:][:-1,:]
odat = ws10.odat[ws10.otheridx,:][1:, ::ws10.nps]
map_ = dot(odat.T, pinv(idat.T, rcond=1e-7))
fmdls.append(map_)
ws10.fmdls_ank_r = fmdls[:]
ws10.fmdls_ank_r_doc = ("Affine. models Ankle-SLIP state -> CoM, subject {},".format(conf.subject))
print "AnkleSLIP affine model computed!"
In [10]:
#ws10.idat_r_an
In [11]:
# old version: base models + initial mappings (new: direct mappings)
if False:
# oidx: selected output dimensions
vidx = len([x for x in ws10.kin_labels_ank if not x.startswith('v_')])
oidx = hstack([arange(ws10.nps), #com_z
arange(ws10.nps*vidx,ws10.nps*(vidx + 3)), #v_com_x ... v_com_z
])
#------------ create right "basic" Floquet model
print "building base floquet models..."
print "right stride: ",
ws10.idat_base_ank_r = ws10.kin_out_ank_r_dt[:, ws10.sct0::ws10.nps] # start from 3rd section!
#(far enough from apex)
ws10.idat_base_ank_r_doc = "(detrended) data at starting section for right Floquet model"
idat = ws10.idat_base_ank_r.copy()
#phi_ref = ws10.phi_r0 + ws10.sct0 * (2.*pi/ws10.nps)
odat_0 = ws10.kin_out_ank_r_dt[:, oidx] # complete odat; including apex (which is "before" section0
#of floquet model)
fmdls = []
for secnr in range(ws10.sct0 + 1, ws10.nps):
odat = odat_0[:, secnr::ws10.nps]
_, maps, _ = fda.fitData(idat, odat, nps=1, nrep=50)
fmdls.append(fda.meanMat(maps))
sys.stdout.write('.')
# append map to section 0 (+2pi)
idat = ws10.kin_out_ank_r_dt[:-1, ws10.sct0::ws10.nps]
odat = odat_0[1:, ::ws10.nps]
_, maps, _ = fda.fitData(idat, odat, nps=1, nrep=50)
fmdls.append(fda.meanMat(maps))
ws10.fmdls_ank_r = fmdls[:]
ws10.fmdls_ank_r_doc = ("Lin. models Ankle-SLIP state -> CoM, subject {}," +
" from sect. r. apex+2 to next apex (incl.)".format(conf.subject))
print "done!"
#------------ create left "basic" Floquet model
print "building base floquet models..."
print "left stride: ",
ws10.idat_base_ank_l = ws10.kin_out_ank_l_dt[:, ws10.sct0::ws10.nps] # start from 3rd section!
#(far enough from apex)
ws10.idat_base_ank_l_doc = "(detrended) data at starting section for left Floquet model"
idat = ws10.idat_base_ank_l.copy()
#phi_ref = ws10.phi_l0 + ws10.sct0 * (2.*pi/ws10.nps)
odat_0 = ws10.kin_out_ank_l_dt[:, oidx] # complete odat; including apex (which is "before" section0
# of floquet model)
fmdls = []
for secnr in range(ws10.sct0 + 1, ws10.nps):
odat = odat_0[:, secnr::ws10.nps]
_, maps, _ = fda.fitData(idat, odat, nps=1, nrep=50)
fmdls.append(fda.meanMat(maps))
sys.stdout.write('.')
# append map to section 0 (+2pi)
idat = ws10.kin_out_ank_l_dt[:-1, ws10.sct0::ws10.nps]
odat = odat_0[1:, ::ws10.nps]
_, maps, _ = fda.fitData(idat, odat, nps=1, nrep=50)
fmdls.append(fda.meanMat(maps))
ws10.fmdls_ank_l = fmdls[:]
ws10.fmdls_ank_l_doc = ("Lin. models Ankle-SLIP state -> CoM, subject {}," +
" from sect. l. apex+2 to next apex (incl.)".format(conf.subject))
print "done!"
In [12]:
ws10.idat_fac_r = hstack([ws1.dataset_full.all_IC_r, ws1.fscore_r])
ws10.idat_fac_r = mi.dt_movingavg(ws10.idat_fac_r, conf.dt_window, conf.dt_medfilter)
odat_0 = ws10.odat
#------------ create right "basic" Floquet model
print "building affine models..."
print "right stride: ",
idat = ws10.idat_fac_r.copy()[ws10.otheridx, :]
fmdls = []
for secnr in range(ws10.sct0, ws10.nps):
odat = ws10.odat[ws10.otheridx, secnr::ws10.nps]
map_ = dot(odat.T, pinv(idat.T, rcond=1e-7))
#_, maps, _ = fda.fitData(idat, odat, nps=1, nrep=50)
fmdls.append(map_)
sys.stdout.write('.')
# append map to section 0 (+2pi)
idat = ws10.idat_fac_r[ws10.otheridx,:][:-1,:]
odat = ws10.odat[ws10.otheridx,:][1:, ::ws10.nps]
map_ = dot(odat.T, pinv(idat.T, rcond=1e-7))
fmdls.append(map_)
ws10.fmdls_fac_r = fmdls[:]
ws10.fmdls_fac_r_doc = ("Affine models Factor-SLIP state -> CoM, subject {},".format(conf.subject))
print "done!"
In [13]:
ws10.idat_aug_r = hstack([ws1.dataset_full.all_IC_r, roll(ws1.dataset_full.s_param_l, 1, axis=0)])
ws10.idat_aug_r = mi.dt_movingavg(ws10.idat_aug_r, conf.dt_window, conf.dt_medfilter)
odat_0 = ws10.odat
#------------ create right "basic" Floquet model
print "building affine models..."
print "right stride: ",
idat = ws10.idat_aug_r.copy()[ws10.otheridx, :]
fmdls = []
for secnr in range(ws10.sct0, ws10.nps):
odat = ws10.odat[ws10.otheridx, secnr::ws10.nps]
map_ = dot(odat.T, pinv(idat.T, rcond=1e-7))
#_, maps, _ = fda.fitData(idat, odat, nps=1, nrep=50)
fmdls.append(map_)
sys.stdout.write('.')
#odat = odat_0[:, secnr::ws10.nps]
#_, maps, _ = fda.fitData(idat, odat, nps=1, nrep=50)
#fmdls.append(fda.meanMat(maps))
#sys.stdout.write('.')
# append map to section 0 (+2pi)
idat = ws10.idat_aug_r[ws10.otheridx,:][:-1, :]
odat = ws10.odat[ws10.otheridx,:][1:, ::ws10.nps]
map_ = dot(odat.T, pinv(idat.T, rcond=1e-7))
fmdls.append(map_)
ws10.fmdls_aug_r = fmdls[:]
ws10.fmdls_aug_r_doc = ("Affine models augmented-SLIP state -> CoM, subject {},".format(conf.subject))
print "done!"
In [14]:
if True: # obsolete code
raw_phases = [x['phi2'] for x in ws1.k.raw_dat]
ws10.phi_apex_r = []
ws10.phi_apex_l = []
for stride in ws10.strides:
ws10.phi_apex_r.append(mod(hstack(ws1.dataset_full.all_phases_r)[stride], 2*pi))
ws10.phi_apex_l.append(mod(hstack(ws1.dataset_full.all_phases_l)[stride], 2*pi))
In [15]:
# ************************************************************************************
# Here: Initial maps from apex -> first section of Floquet models
#
# --- gather required data
if False: # obsolete code
raw_phases = [x['phi2'] for x in ws1.k.raw_dat]
ws10.phi_apex_r = []
ws10.phi_apex_l = []
for stride in ws10.strides:
ws10.phi_apex_r.append(mod(hstack(ws1.dataset_full.all_phases_r)[stride], 2*pi))
ws10.phi_apex_l.append(mod(hstack(ws1.dataset_full.all_phases_l)[stride], 2*pi))
ws10.phi_apex_r_doc = "phase mod 2pi of selected right apex"
ws10.phi_apex_l_doc = "phase mod 2pi of selected left apex"
# --- repeated code -> packed into function
def get_m0(kins, idat_base, phases_list, phi_apex, raw_phases):
"""
computes the mapping from apex to first floquet model section
"""
# --- compute phases of output, cut to match other data
all_ophases = []
for k, aphis in zip(kins, phases_list):
p0 = round((aphis[0] - phi_apex) / (2*pi))
ophases = (arange(len(aphis)) + p0) * 2 * pi + phi_apex
all_ophases.append(ophases)
# --- interpolate data to phase of selected apex
idats = []
for nr, phis in enumerate(all_ophases):
dat = [interp(phis, raw_phases[nr].squeeze(), kins[nr][dim, :]) for dim in range(2, kins[nr].shape[0])]
idats.append(vstack(dat).T)
apexdat = vstack(idats)
apexdat_dt = fda.dt_movingavg(vstack(idats), conf.dt_window, conf.dt_medfilter)
_, maps, _ = fda.fitData(apexdat_dt, idat_base, nps=1, nrep=50)
fmdl = fda.meanMat(maps)
return apexdat, apexdat_dt, fmdl
# --- --- for full state kinematic model
#print "full state Floquet model:"
ws10.apexdat_r = []
ws10.apexdat_r_doc = "all kinematic states at the same phase as selected (right) apex"
ws10.apexdat_r_dt = []
ws10.apexdat_r_dt_doc = "detrended data of apexdat_r"
ws10.fmdl0_r = []
ws10.fmdl0_r_doc = "Lin. models maping full state: apex -> sect. 2, subject {}".format(conf.subject)
ws10.apexdat_l = []
ws10.apexdat_l_doc = "all kinematic states at the same phase as selected (left) apex"
ws10.apexdat_l_dt = []
ws10.apexdat_l_dt_doc = "detrended data of apexdat_l"
ws10.fmdl0_l = []
ws10.fmdl0_l_doc = "Lin. models maping full state: apex -> sect. 2, subject {}".format(conf.subject)
ws10.apexdat_ank_r = []
ws10.apexdat_ank_r_doc = "all ankle-SLIP states at the same phase as selected (right) apex"
ws10.apexdat_ank_r_dt = []
ws10.apexdat_ank_r_dt_doc = "detrended data of apexdat_ank_r"
ws10.fmdl0_ank_r = []
ws10.fmdl0_ank_r_doc = "Lin. models maping ankle-SLIP state: apex -> sect. 2, subject {}".format(conf.subject)
ws10.apexdat_ank_l = []
ws10.apexdat_ank_l_doc = "all ankle-SLIP states at the same phase as selected (left) apex"
ws10.apexdat_ank_l_dt = []
ws10.apexdat_ank_l_dt_doc = "detrended data of apexdat_ank_l"
ws10.fmdl0_ank_l = []
ws10.fmdl0_ank_l_doc = "Lin. models maping ankle-SLIP state: apex -> sect. 2, subject {}".format(conf.subject)
for nr, stride in enumerate(ws10.strides):
print "right stride: full state"
a_dat, a_dat_dt, fmdl = get_m0(ws10.kins_full, ws10.idat_base_r,
ws1.dataset_full.all_phases_r, ws10.phi_apex_r[nr], raw_phases)
ws10.apexdat_r.append(a_dat)
ws10.apexdat_r_dt.append(a_dat_dt)
ws10.fmdl0_r.append(fmdl)
print "right stride: ank SLIP state"
a_dat, a_dat_dt, fmdl = get_m0(ws10.kins_ank, ws10.idat_base_ank_r,
ws1.dataset_full.all_phases_r, ws10.phi_apex_r[nr], raw_phases)
ws10.apexdat_ank_r.append(a_dat)
ws10.apexdat_ank_r_dt.append(a_dat_dt)
ws10.fmdl0_ank_r.append(fmdl)
print "left stride: full state"
a_dat, a_dat_dt, fmdl = get_m0(ws10.kins_full, ws10.idat_base_l,
ws1.dataset_full.all_phases_l, ws10.phi_apex_l[nr], raw_phases)
ws10.apexdat_l.append(a_dat)
ws10.apexdat_l_dt.append(a_dat_dt)
ws10.fmdl0_l.append(fmdl)
print "left stride: ank SLIP state"
a_dat, a_dat_dt, fmdl = get_m0(ws10.kins_ank, ws10.idat_base_ank_l,
ws1.dataset_full.all_phases_l, ws10.phi_apex_l[nr], raw_phases)
ws10.apexdat_ank_l.append(a_dat)
ws10.apexdat_ank_l_dt.append(a_dat_dt)
ws10.fmdl0_ank_l.append(fmdl)
print "initial mappings computed"
In [16]:
# ---- perform prediction of full kinematic floquet models
if not 'p' in ws10.__dict__.keys():
ws10.p = mio.saveable()
ws10.p_doc = "predictions of the floquet models"
# find corresponding indices of the data
dim_lbls = ['com_z', 'v_com_y', 'v_com_z', 'v_com_x',]
idcs = hstack([find(array(ws10.kin_out_r_labels) == x) for x in dim_lbls])
idcs_ank = hstack([find(array(ws10.kin_labels_ank) == x) for x in dim_lbls])
ws10.p.pred_r = []
ws10.p.pred_ank_r = []
ws10.p.pred_l = []
ws10.p.pred_ank_l = []
ws10.p.exp_dt_r = []
ws10.p.exp_dt_l = []
ws10.p.pred_fac_r = []
ws10.p.pred_aug_r = []
ws10.p.phi_vector_r = []
ws10.p.phi_vector_l = []
for nr, stride in enumerate(ws10.strides):
# ---- ---- right side full state
#idat = ws10.apexdat_r_dt[nr][stride, :]
#odat0 = dot(ws10.fmdl0_r[0], idat)
#odats = [dot(mdl, odat0) for mdl in ws10.fmdls_r]
idat = ws10.idat_full_r[stride, :]
odats = [dot(mdl, idat) for mdl in ws10.fmdls_full_r]
#odat0 = dot(ws10.fmdl0_ank_r[0], idat)
#odats = [dot(mdl, odat0) for mdl in ws10.fmdls_ank_r]
# --- --- --- format: (1) initial value, (2) prediction to first section of fmdl, (3) fmdl-predictions
#ws10.p.pred_r.append(vstack([ws10.apexdat_r_dt[nr][stride, idcs], odat0[idcs], odats]))
ws10.p.pred_r.append(vstack([ws10.idat_full_r[stride, :4], odats]))
ws10.p.pred_r[-1][0, 2] = 0 # correct "vz"
ws10.p.pred_r_doc = "prediction of full state (r) based floquet model (w/o baseline)"
# ---- ---- right side ankle SLIP state
#idat = ws10.apexdat_ank_r_dt[nr][stride, :]
#odat0 = dot(ws10.fmdl0_ank_r[0], idat)
#odats = [dot(mdl, odat0) for mdl in ws10.fmdls_ank_r]
odats = [dot(mdl, ws10.idat_ank_r[stride, :]) for mdl in ws10.fmdls_ank_r]
# --- --- --- format: (1) initial value, (2) prediction to first section of fmdl, (3) fmdl-predictions
ws10.p.pred_ank_r.append(vstack([ws10.idat_ank_r[stride, :4],
odats]))
ws10.p.pred_ank_r[-1][0, 2] = 0 # correct "vz"
ws10.p.pred_ank_r_doc = "prediction of ankle-SLIP state (r) based floquet model (w/o baseline)"
# ---- ---- right side factor SLIP state (NOTE: no initial mapping!)
idat = ws10.idat_fac_r[stride, :]
odats = [dot(mdl, idat) for mdl in ws10.fmdls_fac_r]
# --- --- --- format: (1) initial value, (2) prediction to first section of fmdl, (3) fmdl-predictions
ws10.p.pred_fac_r.append(vstack([ws10.idat_fac_r[stride, :4], odats]))
# note: vz [0] is actually a factor state (but unused)
ws10.p.pred_fac_r[-1][0, 2] = 0 # correct vz
ws10.p.pred_fac_r_doc = "prediction of factor-SLIP state (r) based floquet model (w/o baseline)"
# ---- ---- right side augmented SLIP state (NOTE: no initial mapping!)
idat = ws10.idat_aug_r[stride, :]
odats = [dot(mdl, idat) for mdl in ws10.fmdls_aug_r]
# --- --- --- format: (1) initial value, (2) prediction to first section of fmdl, (3) fmdl-predictions
ws10.p.pred_aug_r.append(vstack([ws10.idat_aug_r[stride, :4], odats]))
# note: vz [0] is actually a SLIP parameter (but unused)
ws10.p.pred_aug_r[-1][0, 2] = 0 # correct "vz"
ws10.p.pred_aug_r_doc = "prediction of augmented-SLIP state (r) based floquet model (w/o baseline)"
# experimental data (right): baseline and step
ws10.p.baseline_r = ws10.baseline.copy()
ws10.p.baseline_l = roll(ws10.baseline, ws10.nps//2, axis=0).copy()
# account for belt speed (not accounted for in kinematic data)
vbelt = .5 *( mean(ws1.dataset_full.all_IC_r[:,1]) - ws10.p.baseline_r[0, 1] +
mean(ws1.dataset_full.all_IC_l[:,1]) - ws10.p.baseline_l[0, 1])
ws10.p.baseline_r[:, 1] += vbelt
ws10.p.baseline_l[:, 1] += vbelt
ws10.p.baseline_std_r = ws10.baseline_std.copy()
#baseline = ws10.kin_out_r - ws10.kin_out_r_dt
#baseline_r = vstack([baseline[stride, ws10.nps*idx + ws10.sct0: ws10.nps*(idx+1)]
# for idx in idcs]).T
#ws10.p.baseline_r = vstack([(ws10.apexdat_r[nr] - ws10.apexdat_r_dt[nr])[stride, idcs][newaxis, :],
# baseline_r, baseline[stride + 1, ws10.nps*idcs][newaxis, :]])
#ws10.p.baseline_r_doc = "baseline of CoM dynamics around selected stride (right)"
odat_dt_r = vstack([ws10.kin_out_r_dt[stride, ws10.nps*idx + ws10.sct0: ws10.nps*(idx+1)]
for idx in idcs]).T
ws10.p.exp_dt_r.append(vstack([ws10.idat_full_r[stride, [0,1,3,2]], odat_dt_r,
ws10.kin_out_r_dt[stride + 1, ws10.nps*idcs][newaxis, :]]))
ws10.p.exp_dt_r[-1][0, 2] = 0 # correct "vz"
ws10.p.exp_dt_r_doc = "experimental data at selected stride (right; w/o baseline)"
if False: # currently: no left affine models implemented
# ---- ---- left side
idat = ws10.apexdat_l_dt[nr][stride, :]
odat0 = dot(ws10.fmdl0_l[0], idat)
odats = [dot(mdl, odat0) for mdl in ws10.fmdls_l]
# --- --- --- format: (1) initial value, (2) prediction to first section of fmdl, (3) fmdl-predictions
ws10.p.pred_l.append(vstack([ws10.apexdat_l_dt[nr][stride, idcs], odat0[idcs], odats]))
# ---- ---- left side ankle SLIP state
idat = ws10.apexdat_ank_l_dt[nr][stride, :]
odat0 = dot(ws10.fmdl0_ank_l[0], idat)
odats = [dot(mdl, odat0) for mdl in ws10.fmdls_ank_l]
# --- --- --- format: (1) initial value, (2) prediction to first section of fmdl, (3) fmdl-predictions
ws10.p.pred_ank_l.append(vstack([ws10.apexdat_ank_l_dt[nr][stride, idcs_ank], odat0[idcs_ank], odats]))
ws10.p.pred_ank_r_doc = "prediction of ankle-SLIP state (r) based floquet model (w/o baseline)"
# experimental data (left): baseline and step
baseline = ws10.kin_out_l - ws10.kin_out_l_dt
baseline_l = vstack([baseline[stride, ws10.nps*idx + ws10.sct0: ws10.nps*(idx+1)]
for idx in idcs]).T
ws10.p.baseline_l = vstack([(ws10.apexdat_l[nr] - ws10.apexdat_l_dt[nr])[stride, idcs][newaxis, :],
baseline_l, baseline[stride + 1, ws10.nps*idcs][newaxis, :]])
odat_dt_l = vstack([ws10.kin_out_l_dt[stride, ws10.nps*idx + ws10.sct0: ws10.nps*(idx+1)]
for idx in idcs]).T
ws10.p.exp_dt_l.append(vstack([ws10.apexdat_l_dt[nr][stride, idcs], odat_dt_l,
ws10.kin_out_l_dt[stride + 1, ws10.nps*idcs][newaxis, :]]))
ws10.p.exp_dt_l_doc = "experimental data at selected stride (left; w/o baseline)"
# --- compute phases to which data correspond: [apex, fmdl phases] (required for finding
# corresponding time later)
if False: # old code!
phi_vector_r = hstack([ws10.phi_apex_r[nr] - ws10.phi_r0, linspace(2*pi* float(ws10.sct0) /
float(ws10.nps), 2*pi, len(ws10.fmdls_r) + 1)])
phi_vector_l = hstack([ws10.phi_apex_l[nr] - ws10.phi_r0,
linspace(2*pi* float(ws10.sct0) / float(ws10.nps), 2*pi, len(ws10.fmdls_l) + 1)
+ ws10.phi_apex_l[nr] - ws10.phi_r0 ])
else:
phi_vector_r = linspace(0, 2*pi, len(ws10.fmdls_full_r) + 1)
phi_vector_l = linspace(0, 2*pi, len(ws10.fmdls_full_r) + 1)
ws10.p.phi_vector_r.append(phi_vector_r)
ws10.p.phi_vector_l.append(phi_vector_l)
print "floquet model prediction computed"
In [17]:
idcs
Out[17]:
In [18]:
#ws10.idat_full_r.shape
#odat_dr_r.shape
In [19]:
# create controlled SLIP models
indices_ = ws10.otheridx # control maps are regressed once using all data except those strides to predict
mi.run_nbcells(nb_name, ['4', '4.2a', '4.2b', '4.2c', '4.2d'])
In [20]:
#figure()
#plot(ws10.p.sim_t_r_facs[0], ws10.p.sim_states_r_facs[0][:, 3], 'r')
#plot(sim_t_r, sim_state_r[:, 4], 'r')
In [21]:
# --- run controlled SLIPS and re-sample data (according to phase)
sim_exp_idx = [1,3,4,5] #old: [1,5,3,4]
w = ws1.cslip # shortcut
#w = mio.saveable()
#w.Pp_r, w.Pp_l, w.fac_slip_refstate_r, w.fac_slip_refstate_l
ws10.p.sim_states_l_ank = []
ws10.p.sim_states_r_ank = []
ws10.p.sim_t_l_ank = []
ws10.p.sim_t_r_ank = []
ws10.p.sim_states_l_full = []
ws10.p.sim_states_r_full = []
ws10.p.sim_t_l_full = []
ws10.p.sim_t_r_full = []
ws10.p.sim_states_l_aug = []
ws10.p.sim_states_r_aug = []
ws10.p.sim_t_l_aug = []
ws10.p.sim_t_r_aug = []
ws10.p.sim_states_l_facs = []
ws10.p.sim_states_r_facs = []
ws10.p.sim_t_l_facs = []
ws10.p.sim_t_r_facs = []
ws10.p.sim_states_l_uc = []
ws10.p.sim_states_r_uc = []
ws10.p.sim_t_l_uc = []
ws10.p.sim_t_r_uc = []
ws10.p.sim_states_l_per = []
ws10.p.sim_states_r_per = []
ws10.p.sim_t_l_per = []
ws10.p.sim_t_r_per = []
# prepare Ankle-SLIP data
# --- --- create input data (code copied from original model definition in block 4
ws1.k.selection = ['com_x', 'com_y', 'com_z', 'r_anl_y - com_y', 'r_anl_z - com_z', 'r_anl_x - com_x']
dataset_r = build_dataset(ws1.k, ws1.SlipData, dt_window=conf.dt_window, dt_median=conf.dt_medfilter)
ws1.k.selection = ['com_x', 'com_y', 'com_z', 'l_anl_y - com_y', 'l_anl_z - com_z', 'l_anl_x - com_x']
dataset_l= build_dataset(ws1.k, ws1.SlipData, dt_window=conf.dt_window, dt_median=conf.dt_medfilter)
ws1.k_doc = "KinData object for s:" + str(ws1.k.subject) + " t:" + str(ws1.k.ttype) + " with left ankle"
ankleSLIP_states_r_dt = fda.dt_movingavg(dataset_r.all_kin_r[:, reord_idx(dataset_r.kin_labels)],
conf.dt_window, conf.dt_medfilter)
ankleSLIP_states_l_dt = fda.dt_movingavg(dataset_l.all_kin_l[:, reord_idx(dataset_l.kin_labels)],
conf.dt_window, conf.dt_medfilter)
# prepare fullstate-SLIP data
ws1.k.selection = ws1.full_markers
dataset_full = build_dataset(ws1.k, ws1.SlipData, dt_window=conf.dt_window, dt_median=conf.dt_medfilter)
ws1.k_doc = "KinData object for s:" + str(ws1.k.subject) + " t:" + str(ws1.k.ttype) + " (full state)"
fullSLIP_states_r_dt = fda.dt_movingavg(dataset_full.all_kin_r[:, reord_idx(dataset_full.kin_labels)],
conf.dt_window, conf.dt_medfilter)
fullSLIP_states_l_dt = fda.dt_movingavg(dataset_full.all_kin_l[:, reord_idx(dataset_full.kin_labels)],
conf.dt_window, conf.dt_medfilter)
# prepare reference states
for nr, stride in enumerate(ws10.strides):
com_apex_r = ws1.dataset_full.all_IC_r[stride] # ws10.apexdat_r[ws10.step, idcs][:3]
com_apex_l = ws1.dataset_full.all_IC_l[stride] # ws10.apexdat_l[ws10.step, idcs][:3]
# --- run factor SLIP
# --- --- create input data
fscore_r_dt = mi.dt_movingavg(ws1.fscore_r, conf.dt_window, conf.dt_medfilter)
fscore_l_dt = mi.dt_movingavg(ws1.fscore_l, conf.dt_window, conf.dt_medfilter)
idat_fSLIP_r = hstack([com_apex_r, fscore_r_dt[stride, :]])
idat_fSLIP_l = hstack([com_apex_l, fscore_l_dt[stride, :]])
# --- --- create dynamical systems
facSLIP_r = get_auto_sys(w.Pp_r, w.Pp_l, w.fac_slip_refstate_r,
w.fac_slip_refstate_l, w.fac_slip_ctrl_r, w.fac_slip_ctrl_l,
w.fac_slip_dprop_r, w.fac_slip_dprop_l, {'m': ws1.cslip.mass, 'g':-9.81},
full_info=True)
facSLIP_l = get_auto_sys(w.Pp_l, w.Pp_r, w.fac_slip_refstate_l,
w.fac_slip_refstate_r, w.fac_slip_ctrl_l, w.fac_slip_ctrl_r,
w.fac_slip_dprop_l, w.fac_slip_dprop_r, {'m': ws1.cslip.mass, 'g':-9.81},
full_info=True)
# --- --- run simulations
fsr, (sim_t_r, sim_state_r) = facSLIP_r(idat_fSLIP_r)
fsl, (sim_t_l, sim_state_l) = facSLIP_l(idat_fSLIP_l)
sim_t_r_facs = sim_t_r
sim_t_l_facs = sim_t_l
sim_state_r_facs = sim_state_r[:, sim_exp_idx]
sim_state_l_facs = sim_state_l[:, sim_exp_idx]
ws10.p.sim_t_r_facs.append(sim_t_r_facs)
ws10.p.sim_t_l_facs.append(sim_t_l_facs)
ws10.p.sim_states_r_facs.append(sim_state_r_facs)
ws10.p.sim_states_l_facs.append(sim_state_l_facs)
# --- END run factor SLIP
# --- run ankle SLIP
idat_ankSLIP_r = hstack([com_apex_r, ankleSLIP_states_r_dt[stride, 3:]])
idat_ankSLIP_l = hstack([com_apex_l, ankleSLIP_states_l_dt[stride, 3:]])
# --- --- create dynamical systems
ankSLIP_r = get_auto_sys(w.Pp_r, w.Pp_l, w.ankle_slip_refstate_r,
w.ankle_slip_refstate_l, w.ankle_slip_ctrl_r, w.ankle_slip_ctrl_l,
w.ankle_slip_dprop_r, w.ankle_slip_dprop_l, {'m': ws1.cslip.mass, 'g':-9.81},
full_info=True)
ankSLIP_l = get_auto_sys(w.Pp_l, w.Pp_r, w.ankle_slip_refstate_l,
w.ankle_slip_refstate_r, w.ankle_slip_ctrl_l, w.ankle_slip_ctrl_r,
w.ankle_slip_dprop_l, w.ankle_slip_dprop_r, {'m': ws1.cslip.mass, 'g':-9.81},
full_info=True)
# --- --- run simulations
fsr, (sim_t_r, sim_state_r) = ankSLIP_r(idat_ankSLIP_r)
fsl, (sim_t_l, sim_state_l) = ankSLIP_l(idat_ankSLIP_l)
sim_t_r_ank = sim_t_r
sim_t_l_ank = sim_t_l
sim_state_r_ank = sim_state_r[:, sim_exp_idx]
sim_state_l_ank = sim_state_l[:, sim_exp_idx]
ws10.p.sim_t_r_ank.append(sim_t_r_ank)
ws10.p.sim_t_l_ank.append(sim_t_l_ank)
ws10.p.sim_states_r_ank.append(sim_state_r_ank)
ws10.p.sim_states_l_ank.append(sim_state_l_ank)
# --- END ankle SLIP
# --- run fullstate SLIP
idat_fullSLIP_r = hstack([com_apex_r, fullSLIP_states_r_dt[stride, 3:]])
idat_fullSLIP_l = hstack([com_apex_l, fullSLIP_states_l_dt[stride, 3:]])
# --- --- create dynamical systems
fullSLIP_r = get_auto_sys(w.Pp_r, w.Pp_l, w.full_slip_refstate_r,
w.full_slip_refstate_l, w.full_slip_ctrl_r, w.full_slip_ctrl_l,
w.full_slip_dprop_r, w.full_slip_dprop_l, {'m': ws1.cslip.mass, 'g':-9.81},
full_info=True)
fullSLIP_l = get_auto_sys(w.Pp_l, w.Pp_r, w.full_slip_refstate_l,
w.full_slip_refstate_r, w.full_slip_ctrl_l, w.full_slip_ctrl_r,
w.full_slip_dprop_l, w.full_slip_dprop_r, {'m': ws1.cslip.mass, 'g':-9.81},
full_info=True)
# --- --- run simulations
fsr, (sim_t_r, sim_state_r) = fullSLIP_r(idat_fullSLIP_r)
fsl, (sim_t_l, sim_state_l) = fullSLIP_l(idat_fullSLIP_l)
sim_t_r_full = sim_t_r
sim_t_l_full = sim_t_l
sim_state_r_full = sim_state_r[:, sim_exp_idx]
sim_state_l_full = sim_state_l[:, sim_exp_idx]
ws10.p.sim_t_r_full.append(sim_t_r_full)
ws10.p.sim_t_l_full.append(sim_t_l_full)
ws10.p.sim_states_r_full.append(sim_state_r_full)
ws10.p.sim_states_l_full.append(sim_state_l_full)
# --- END fullstate SLIP
# --- run augmented SLIP
# --- --- create input data
idat_augSLIP_r = hstack([com_apex_r, ws1.dataset_full.s_param_l[stride - 1, :]])
idat_augSLIP_l = hstack([com_apex_l, ws1.dataset_full.s_param_r[stride, :]])
# --- --- create dynamical systems
augSLIP_r = get_auto_sys(w.Pp_r, w.Pp_l, w.aug_slip_refstate_r,
w.aug_slip_refstate_l, w.aug_slip_ctrl_r, w.aug_slip_ctrl_l,
w.aug_slip_dprop_r, w.aug_slip_dprop_l, {'m': ws1.cslip.mass, 'g':-9.81},
full_info=True)
augSLIP_l = get_auto_sys(w.Pp_l, w.Pp_r, w.aug_slip_refstate_l,
w.aug_slip_refstate_r, w.aug_slip_ctrl_l, w.aug_slip_ctrl_r,
w.aug_slip_dprop_l, w.aug_slip_dprop_r, {'m': ws1.cslip.mass, 'g':-9.81},
full_info=True)
# --- --- run simulations
fsr, (sim_t_r, sim_state_r) = augSLIP_r(idat_augSLIP_r)
fsl, (sim_t_l, sim_state_l) = augSLIP_l(idat_augSLIP_l)
sim_t_r_aug = sim_t_r
sim_t_l_aug = sim_t_l
sim_state_r_aug = sim_state_r[:, sim_exp_idx]
sim_state_l_aug = sim_state_l[:, sim_exp_idx]
ws10.p.sim_t_r_aug.append(sim_t_r_aug)
ws10.p.sim_t_l_aug.append(sim_t_l_aug)
ws10.p.sim_states_r_aug.append(sim_state_r_aug)
ws10.p.sim_states_l_aug.append(sim_state_l_aug)
# --- --- compute phase of the systems, resample output
#print "Augmented-SLIP simulations computed"
# --- END augmented SLIP
# --- run uncontrolled SLIP
# this is a clone of ankle-SLIP, except that all states and maps are set to zero
idat_ucSLIP_r = hstack([com_apex_r, 0*ankleSLIP_states_r_dt[stride, 3:]])
idat_ucSLIP_l = hstack([com_apex_l, 0*ankleSLIP_states_l_dt[stride, 3:]])
# --- --- create dynamical systems
ucSLIP_r = get_auto_sys(w.Pp_r, w.Pp_l, w.ankle_slip_refstate_r,
w.ankle_slip_refstate_l, 0*w.ankle_slip_ctrl_r,0* w.ankle_slip_ctrl_l,
0*w.ankle_slip_dprop_r, 0*w.ankle_slip_dprop_l, {'m': ws1.cslip.mass, 'g':-9.81},
full_info=True)
ucSLIP_l = get_auto_sys(w.Pp_l, w.Pp_r, w.ankle_slip_refstate_l,
w.ankle_slip_refstate_r, 0*w.ankle_slip_ctrl_l, 0*w.ankle_slip_ctrl_r,
0*w.ankle_slip_dprop_l, 0*w.ankle_slip_dprop_r, {'m': ws1.cslip.mass, 'g':-9.81},
full_info=True)
# --- --- run simulations
uc_sr = False
uc_sl = False
try:
fsr, (sim_t_r, sim_state_r) = ucSLIP_r(idat_ucSLIP_r)
except ValueError:
uc_sr = True
print "UC SLIP: step", stride, " skipped (R)"
try:
fsl, (sim_t_l, sim_state_l) = ucSLIP_l(idat_ucSLIP_l)
except ValueError:
uc_sr = True
print "UC SLIP: step", stride, " skipped (L)"
sim_t_r_uc = sim_t_r
sim_t_l_uc = sim_t_l
sim_state_r_uc = sim_state_r[:, sim_exp_idx]
sim_state_l_uc = sim_state_l[:, sim_exp_idx]
ws10.p.sim_t_r_uc.append(sim_t_r_uc)
ws10.p.sim_t_l_uc.append(sim_t_l_uc)
ws10.p.sim_states_r_uc.append(sim_state_r_uc)
ws10.p.sim_states_l_uc.append(sim_state_l_uc)
# --- --- compute phase of the systems, resample output
#print "uncontrolled-SLIP simulations computed"
fsr, (sim_t_r, sim_state_r) = ucSLIP_r(w.ankle_slip_refstate_r)
fsl, (sim_t_l, sim_state_l) = ucSLIP_l(w.ankle_slip_refstate_l)
sim_t_r_per = sim_t_r
sim_t_l_per = sim_t_l
sim_state_r_per = sim_state_r[:, sim_exp_idx]
sim_state_l_per = sim_state_l[:, sim_exp_idx]
ws10.p.sim_t_r_per = sim_t_r_per
ws10.p.sim_t_l_per = sim_t_l_per
ws10.p.sim_states_r_per = sim_state_r_per
ws10.p.sim_states_l_per = sim_state_l_per
#print "SLIP periodic motion simulated"
# --- END uncontrolled SLIP
print "simulation for stride " + str(stride) + " completed"
print "done"
In [22]:
# --- identify repetition and step number in that repetition
rsteps = []
reps = []
for stride in ws10.strides:
ts = 0
for rep, x in enumerate(ws1.dataset_full.all_phases_r):
if ts + len(x) < stride:
ts += len(x)
else:
break
rstep = stride - ts
rsteps.append(rstep)
reps.append(rep)
print "rep:", reps, "rstep:", rsteps
In [23]:
# --- --- obtain raw phases -> build (small) data set
ws1.k.selection=['com_x', 'com_y', 'com_z', 'l_anl_x - com_x'] # just any small selection
_ = ws1.k.make_1D(ws10.nps, phases_list = ws1.dataset_full.all_phases_r)
out_phases_r = ws1.k.i_phases[rep].copy()
_ = ws1.k.make_1D(ws10.nps, phases_list = ws1.dataset_full.all_phases_l)
out_phases_l = ws1.k.i_phases[rep].copy()
sampletime = linspace(0, 240, len(ws1.k.raw_dat[rep]['phi2'].squeeze()), endpoint=False)
ws10.p.sim_r_facs = []
ws10.p.sim_l_facs = []
ws10.p.sim_r_full = []
ws10.p.sim_l_full = []
ws10.p.sim_r_ank = []
ws10.p.sim_l_ank = []
ws10.p.sim_r_aug = []
ws10.p.sim_l_aug = []
ws10.p.sim_r_uc = []
ws10.p.sim_l_uc = []
ws10.p.sim_r_per = []
ws10.p.sim_l_per = []
ws10.p.kintime_r = []
ws10.p.kintime_l = []
for nr, stride in enumerate(ws10.strides):
# --- --- time vector for 'right' stride
# --- --- --- align subsequent strides!
if len(ws10.p.kintime_r):
dt = ws10.p.kintime_r[-1][-1]
else:
dt = 0
dt += ws10.tspace
phi0_r = ws1.dataset_full.all_phases_r[reps[nr]][rsteps[nr]]
phi_r_full = ws10.p.phi_vector_r[nr] - ws10.phi_apex_r[nr] + ws10.phi_r0 + phi0_r
kintime_r0 = interp(phi_r_full, ws1.k.raw_dat[reps[nr]]['phi2'].squeeze(), sampletime)
ws10.p.kintime_r.append(kintime_r0 - kintime_r0[0] + dt)
# --- --- time vector for 'left' stride
phi0_l = ws1.dataset_full.all_phases_l[reps[nr]][rsteps[nr]]
phi_l_full = ws10.p.phi_vector_l[nr] - ws10.phi_apex_l[nr] + ws10.phi_r0 + phi0_l
kintime_l0 = interp(phi_l_full, ws1.k.raw_dat[reps[nr]]['phi2'].squeeze(), sampletime)
ws10.p.kintime_l.append(kintime_l0 - kintime_r0[0] + dt) # relative to *right* apex!
print "time obtained"
# --- --- interpolate simulation data
if True: # block not yet ready
# --- --- --- NOTE : ALSO ADD SHIFT TO RIGHT STRIDE (NEW!) --- ---
ws10.p.sim_r_facs.append( vstack([interp(ws10.p.kintime_r[-1] - ws10.p.kintime_r[-1][0],
ws10.p.sim_t_r_facs[nr],
ws10.p.sim_states_r_facs[nr][:, dim], nan, nan)
for dim in range(ws10.p.sim_states_r_facs[nr].shape[1])]).T )
ws10.p.sim_l_facs.append( vstack([interp(ws10.p.kintime_l[-1] - ws10.p.kintime_l[-1][0],
ws10.p.sim_t_l_facs[nr],
ws10.p.sim_states_l_facs[nr][:, dim], nan, nan)
for dim in range(ws10.p.sim_states_l_facs[nr].shape[1])]).T)
ws10.p.sim_r_full.append( vstack([interp(ws10.p.kintime_r[-1] - ws10.p.kintime_r[-1][0],
ws10.p.sim_t_r_full[nr],
ws10.p.sim_states_r_full[nr][:, dim], nan, nan)
for dim in range(ws10.p.sim_states_r_full[nr].shape[1])]).T )
ws10.p.sim_l_full.append( vstack([interp(ws10.p.kintime_l[-1] - ws10.p.kintime_l[-1][0],
ws10.p.sim_t_l_full[nr],
ws10.p.sim_states_l_full[nr][:, dim], nan, nan)
for dim in range(ws10.p.sim_states_l_full[nr].shape[1])]).T)
ws10.p.sim_r_ank.append( vstack([interp(ws10.p.kintime_r[-1]- ws10.p.kintime_r[-1][0],
ws10.p.sim_t_r_ank[nr],
ws10.p.sim_states_r_ank[nr][:, dim], nan, nan)
for dim in range(ws10.p.sim_states_r_ank[nr].shape[1])]).T)
ws10.p.sim_l_ank.append( vstack([interp(ws10.p.kintime_l[-1] - ws10.p.kintime_l[-1][0],
ws10.p.sim_t_l_ank[nr],
ws10.p.sim_states_l_ank[nr][:, dim], nan, nan)
for dim in range(ws10.p.sim_states_l_ank[nr].shape[1])]).T)
ws10.p.sim_r_aug.append( vstack([interp(ws10.p.kintime_r[-1] - ws10.p.kintime_r[-1][0],
ws10.p.sim_t_r_aug[nr],
ws10.p.sim_states_r_aug[nr][:, dim], nan, nan)
for dim in range(ws10.p.sim_states_r_aug[nr].shape[1])]).T)
ws10.p.sim_l_aug.append( vstack([interp(ws10.p.kintime_l[-1] - ws10.p.kintime_l[-1][0],
ws10.p.sim_t_l_aug[nr],
ws10.p.sim_states_l_aug[nr][:, dim], nan, nan)
for dim in range(ws10.p.sim_states_l_aug[nr].shape[1])]).T)
ws10.p.sim_r_uc.append( vstack([interp(ws10.p.kintime_r[-1] - ws10.p.kintime_r[-1][0],
ws10.p.sim_t_r_uc[nr],
ws10.p.sim_states_r_uc[nr][:, dim], nan, nan)
for dim in range(ws10.p.sim_states_r_uc[nr].shape[1])]).T)
ws10.p.sim_l_uc.append( vstack([interp(ws10.p.kintime_l[-1] - ws10.p.kintime_l[-1][0],
ws10.p.sim_t_l_uc[nr],
ws10.p.sim_states_l_uc[nr][:, dim], nan, nan)
for dim in range(ws10.p.sim_states_l_uc[nr].shape[1])]).T)
ws10.p.sim_r_per.append( vstack([interp(ws10.p.kintime_r[-1] - ws10.p.kintime_r[-1][0],
ws10.p.sim_t_r_per,
ws10.p.sim_states_r_per[:, dim], nan, None)
for dim in range(ws10.p.sim_states_r_per.shape[1])]).T)
ws10.p.sim_l_per.append( vstack([interp(ws10.p.kintime_l[-1] - ws10.p.kintime_l[-1][0],
ws10.p.sim_t_l_per,
ws10.p.sim_states_l_per[:, dim], nan, nan)
for dim in range(ws10.p.sim_states_l_per.shape[1])]).T)
print "simulation data interpolated"
In [24]:
def plotband(x, y, ups, downs, color='#0067ea', alpha=.25, **kwargs):
"""
plots a line surrounded by a ribbon of the same color, but semi-transparent
:args:
x (N-by-1): positions on horizontal axis
y (N-by-1): corresponding vertical values of the (center) line
ups (N-by-1): upper edge of the ribbon
downs (N-by-1): lower edge of the ribbon
:returns:
[line, patch] returns of underlying "plot" and "fill_between" function
"""
pt1 = plot(x, y, color, **kwargs )
pt2 = fill_between(x, ups, downs, color='None', facecolor=color, lw=0, alpha=alpha)
return [pt1, pt2]
In [40]:
close('all')
In [41]:
# cell_ID NCommfigure
# MISSING: "FACTOR MODEL" PREDICTION!
%config InlineBackend.close_figures = False
# my new standard colors from "snippets.ipynb"
colors2 = ['#000000', '#106aa4', '#43bf3c', '#ff7f00', '#ef0a28', '#5f2e82',
'#8f8f8f', '#92bee3', '#a1df80', '#fdaf5f', '#fb8987', '#baa2c5',]
import libshai.util as ut
#reload(ut)
import matplotlib.gridspec as gridspec
import matplotlib.font_manager as fmt
FM = fmt.FontManager()
#fnt = FM.findfont('Times New Roman') # for Proc R Soc
fnt = FM.findfont('Arial') # for NComm
fp = fmt.FontProperties(fname=fnt)
fp.set_size(9.0)
gs1 = gridspec.GridSpec(3, 5, wspace=.07, hspace=.05, left=.1, right=.9, bottom=.0375, top=.80) #was: .835)
#figure(figsize=(6.83, 3)) # ProcRSoc
fig = figure(figsize=(7.03, 5)) # NComm, twocolumn
#fig = figure(figsize=(3.4, 3)) # NComm, single
#ax0 = subplot(1,1,1,frameon=False)
#figure(figsize=(16,8))
#create axes and handles
#axes_a = [fig.add_subplot(3, 1, x+1) for x in range(3)]
#axes_b = [ax.twinx() for ax in axes_a]
#axes_c1 = [ax.twiny() for ax in axes_a]
#axes_c2 = [ax.twiny() for ax in axes_b]
axes_a = [fig.add_subplot(gs1[x, :4]) for x in range(3)]
axes_b = [ax.twinx() for ax in axes_a]
axes_c1 = [fig.add_subplot(gs1[x, 4], sharey=axes_a[x]) for x in range(3)]
axes_c2 = [fig.add_subplot(gs1[x, 4], sharey=axes_b[x], sharex=axes_c1[x], frameon=False) for x in range(3)]
for ax in axes_c1 + axes_c2:
ax.yaxis.tick_right()
#axes_c2 = [ax.twiny() for ax in axes_b]
stepping = 2
for nr, stride in enumerate(ws10.strides):
for ax, dim in enumerate([0,3,1]): #range(3):
lbl = None
# --- "full data" axis
# --- --- experimental and affine model data
#axes_a[dim].plot(ws10.p.kintime_r[nr], ws10.p.baseline_r[:, dim], '-',
# color=colors2[5], linewidth=2, label=lbl)
#axes_a[dim].plot(ws10.p.kintime_r[nr], ws10.p.sim_r_per[nr][:, dim], color=colors2[11],
# linewidth=2, label=lbl)
axes_a[ax].plot(ws10.p.kintime_r[nr][::stepping],
ws10.p.pred_r[nr][::stepping, dim] + ws10.p.baseline_r[::stepping, dim],
',',
color=colors2[0], label=lbl)
axes_a[ax].plot(ws10.p.kintime_r[nr][::stepping],
ws10.p.pred_ank_r[nr][::stepping, dim] + ws10.p.baseline_r[::stepping, dim],
',',
color=colors2[2], label=lbl)
axes_a[ax].plot(ws10.p.kintime_r[nr][::stepping],
ws10.p.pred_fac_r[nr][::stepping, dim] + ws10.p.baseline_r[::stepping, dim],
',',
color=colors2[1], label=lbl)
axes_a[ax].plot(ws10.p.kintime_r[nr][::stepping],
ws10.p.pred_aug_r[nr][::stepping, dim] + ws10.p.baseline_r[::stepping, dim],
',',
color=colors2[3], label=lbl)
# --- --- simulated data
axes_a[ax].plot(ws10.p.kintime_r[nr], ws10.p.sim_r_full[nr][:, dim],'-', lw=.75,
color=colors2[0], label=lbl)
axes_a[ax].plot(ws10.p.kintime_r[nr], ws10.p.sim_r_facs[nr][:, dim],'-', lw=.75,
color=colors2[1], label=lbl)
axes_a[ax].plot(ws10.p.kintime_r[nr], ws10.p.sim_r_ank[nr][:, dim], '-', lw=.75,
color=colors2[2], label=lbl)
axes_a[ax].plot(ws10.p.kintime_r[nr], ws10.p.sim_r_uc[nr][:, dim], '-', lw=.75,
color=colors2[11], label=lbl)
axes_a[ax].plot(ws10.p.kintime_r[nr], ws10.p.sim_r_aug[nr][:, dim], '-', lw=.75,
color=colors2[3], label=lbl)
# --- --- experimental data
axes_a[ax].plot(ws10.p.kintime_r[nr], ws10.p.exp_dt_r[nr][:, dim] + ws10.p.baseline_r[:, dim],'-',
color=colors2[4], linewidth=1, label=lbl)
# --- Baseline Axis
if nr==0:
axes(axes_c1[ax])
plotband(ws10.p.kintime_r[nr], ws10.p.baseline_r[:, dim],
ws10.p.baseline_r[:, dim] + ws10.p.baseline_std_r[:, dim],
ws10.p.baseline_r[:, dim] - ws10.p.baseline_std_r[:, dim], alpha=.175, color=colors2[5], linewidth=1)
#axes_c1[ax].plot(ws10.p.kintime_r[nr], ws10.p.baseline_r[:, dim], '-',
# color=colors2[5], linewidth=2, label=lbl)
axes_c1[ax].plot(ws10.p.kintime_r[nr], ws10.p.sim_r_per[nr][:, dim], color=colors2[11],
linewidth=1, label=lbl)
# +- stds
#axes_c1[ax].plot(ws10.p.kintime_r[nr], ws10.p.baseline_r[:, dim] +
# ws10.p.baseline_std_r[:, dim], '--',
# color=colors2[5], linewidth=1, label=lbl)
#axes_c1[ax].plot(ws10.p.kintime_r[nr], ws10.p.baseline_r[:, dim] -
# ws10.p.baseline_std_r[:, dim], '--',
# color=colors2[5], linewidth=1, label=lbl)
if nr==0:
axes(axes_c2[ax])
plotband(ws10.p.kintime_r[nr], 0 * ws10.p.baseline_r[:, dim], ws10.p.baseline_std_r[:, dim],
-ws10.p.baseline_std_r[:, dim], alpha=.175, color=colors2[5], linewidth=1 )
#axes_c2[ax].plot(ws10.p.kintime_r[nr], 0 * ws10.p.baseline_r[:, dim], '-',
# color=colors2[5], linewidth=2, label=lbl)
## +- stds
#axes_c2[ax].plot(ws10.p.kintime_r[nr], ws10.p.baseline_std_r[:, dim], '--',
# color=colors2[5], linewidth=1, label=lbl)
#
# axes_c2[ax].plot(ws10.p.kintime_r[nr], -ws10.p.baseline_std_r[:, dim], '--',
# color=colors2[5], linewidth=1, label=lbl)
axes_c2[ax].plot(ws10.p.kintime_r[nr], ws10.p.sim_r_per[nr][:, dim] - ws10.p.baseline_r[:, dim],
color=colors2[11], linewidth=1, label=lbl)
# --- "difference" axis
# --- --- affine models
# -> moved to "Baseline Axis" -> further, this is not the same for all steps because of different
# step lengths but same duration of reference motion
#axes_b[dim].plot(ws10.p.kintime_r[nr], ws10.p.sim_r_per[nr][:, dim] - ws10.p.baseline_r[:, dim],
# color=colors2[11], linewidth=2, label=lbl)
#lbl = 'baseline' if not nr else None
#ax1.plot(ws10.p.kintime_r[nr], ws10.p.baseline_r[:, dim],color='#555555', linewidth=4, label=lbl)
#lbl = 'SLIP baseline' if not nr else None
#ax1.plot(ws10.p.kintime_r[nr], ws10.p.sim_r_per[nr][:, dim], '#aaaaaa', linewidth=3, label=lbl)
axes_b[ax].plot(ws10.p.kintime_r[nr][::stepping],
ws10.p.pred_r[nr][::stepping, dim], ',',
color=colors2[0], label=lbl, linewidth=1)
# MISSING: FACTOR MODEL PREDICTION
#axes_b[dim].plot(ws10.p.kintime_r[nr], ws10.p.pred_r[nr][:, dim], color=colors2[0], label=lbl,
# linewidth=1)
axes_b[ax].plot(ws10.p.kintime_r[nr][::stepping],
ws10.p.pred_ank_r[nr][::stepping, dim], ',',
color=colors2[2], label=lbl, linewidth=1)
axes_b[ax].plot(ws10.p.kintime_r[nr][::stepping],
ws10.p.pred_fac_r[nr][::stepping, dim], ',',
color=colors2[1], label=lbl, linewidth=1)
axes_b[ax].plot(ws10.p.kintime_r[nr][::stepping],
ws10.p.pred_aug_r[nr][::stepping, dim], ',',
color=colors2[3], label=lbl, linewidth=1)
# --- --- simulated data
axes_b[ax].plot(ws10.p.kintime_r[nr], ws10.p.sim_r_full[nr][:, dim] - ws10.p.baseline_r[:, dim],
'-', color=colors2[0], label=lbl, linewidth=.75)
axes_b[ax].plot(ws10.p.kintime_r[nr], ws10.p.sim_r_facs[nr][:, dim] - ws10.p.baseline_r[:, dim],
'-', color=colors2[1], label=lbl, linewidth=.75)
axes_b[ax].plot(ws10.p.kintime_r[nr], ws10.p.sim_r_ank[nr][:, dim] - ws10.p.baseline_r[:, dim],
'-', color=colors2[2], label=lbl, linewidth=.75)
axes_b[ax].plot(ws10.p.kintime_r[nr], ws10.p.sim_r_aug[nr][:, dim] - ws10.p.baseline_r[:, dim],
'-', color=colors2[3], label=lbl, linewidth=.75)
axes_b[ax].plot(ws10.p.kintime_r[nr], ws10.p.sim_r_uc[nr][:, dim] - ws10.p.baseline_r[:, dim],'-',
color=colors2[11], label=lbl, lw=.75)
# --- --- experimental data
axes_b[ax].plot(ws10.p.kintime_r[nr], ws10.p.exp_dt_r[nr][:, dim], '-',
color=colors2[4], linewidth=1, label=lbl)
# horizontal line at 0
#axes_b[dim].plot(ws10.p.kintime_r[nr], 0*ws10.p.exp_dt_r[nr][:, dim], '-',
# color='k', linewidth=1, label=lbl)
# skip uncontrolled SLIP
#lbl = 'uncontrolled SLIP' if not nr else None
#label=lbl)
axes_b[0].set_ylim(-0.05,0.01)
axes_a[0].set_ylim(.84,1.02)
for axnr in range(3):
for tr in ws10.p.kintime_r:
axes_a[axnr].plot([tr[0],tr[0]],[-5, 5], '-', color='#000000', lw=2)
pass
In [42]:
# create legend axis
figure(fig.number)
#figure()
axl = axes([.05, .825, .9, .165], frameon=False)
#axl.plot(randn(100))
axl.set_xticks([])
axl.set_yticks([])
# "italic" font - does not work :'(
fpi = fmt.FontProperties(fname=fnt)
fpi.set_size(9.0)
fpi.set_style('oblique')
# first column
#axl.plot([0, 0.2], [0, 0], '-', color=colors2[11])
#axl.text(0.25, 0, 'periodic parameter SLIP', fontproperties=fp, va='center')
axl.plot([2.5, 2.7], [1.3, 1.3], '-', color=colors2[5], lw=1)
axl.text(2.75, 1.3, '(locally) average motion', fontproperties=fp, va='center')
axl.plot([0.5, 0.7], [1.3, 1.3], '-', color=colors2[4])
axl.text(0.75, 1.3, 'experimental data', fontproperties=fp, va='center')
# second column
axl.text(1.4, 0, 'simulated models', fontproperties=fpi, va='center' )
axl.plot([0.5, 0.7], [-1, -1], '-', color=colors2[0])
axl.text(0.75, -1, 'Full state SLIP', fontproperties=fp, va='center')
axl.plot([0.5, 0.7], [-2, -2], '-', color=colors2[1])
axl.text(0.75, -2, 'Factor-SLIP', fontproperties=fp, va='center')
axl.plot([2, 2.2], [-1, -1], '-', color=colors2[2])
axl.text(2.25, -1, 'Ankle-SLIP', fontproperties=fp, va='center')
axl.plot([2, 2.2], [-2, -2], '-', color=colors2[3])
axl.text(2.25, -2, 'Augmented-SLIP', fontproperties=fp, va='center')
axl.plot([0.5, 0.7], [-3, -3], '-', color=colors2[11])
axl.text(0.75, -3, 'periodic parameter SLIP', fontproperties=fp, va='center')
# third column
axl.text(4.2, 0, 'linear models', fontproperties=fpi, va='center' )
axl.plot([3.5, 3.6, 3.7], [-1, -1, -1], '.', markersize=2, color=colors2[0])
axl.text(3.75, -1, 'Full state', fontproperties=fp, va='center')
axl.plot([3.5, 3.6, 3.7], [-2, -2, -2], '.', markersize=2, color=colors2[1])
axl.text(3.75, -2, 'CoM and factors', fontproperties=fp, va='center')
axl.plot([4.7, 4.8, 4.9], [-1, -1, -1], '.', markersize=2, color=colors2[2])
axl.text(4.95, -1, 'CoM and ankle state', fontproperties=fp, va='center')
axl.plot([4.7, 4.8, 4.9], [-2, -2, -2], '.', markersize=2, color=colors2[3])
axl.text(4.95, -2, 'Augmented SLIP state', fontproperties=fp, va='center')
axl.set_xlim(.5,6)
axl.set_ylim(-5, 1.65)
#savefig('tmp/tmp.pdf')
Out[42]:
In [43]:
# make font in math the same as in text
import matplotlib as mpl
mpl.rcParams['mathtext.default'] = 'regular' # this is the key
In [44]:
print '{:.3f}'.format(ws10.p.sim_t_r_per[-1])
In [45]:
ylims_a
Out[45]:
In [46]:
# subject 2 (?), strides 1400 - 1403
#yticks_a = [ [.9, .95, 1.], [-.1, -.05, 0, .05, .1], [2.5, 2.7, 2.9] ]
#yticks_b = [ [-.01,0,.01], [-.05, 0, .05], [-0.05, 0, .05] ]
#ylims_a = [(.89,1.12), (-.14,.29), (2.4,3.6)]
#ylims_b = [(-0.07,0.015),(-.25,.075) , (-.3,.10)]
#subject 1, strides 1200-1203
if conf.subject == 1:
ylims_a = [(.82, 1.08), (-.14, .34), (2.4, 3.5)]
ylims_b = [(-0.07, 0.0175),(-.3, .0975) , (-.35, .085)]
yticks_a = [ [.85, .9, .95], [-.1, -.05, 0, .05, .1], [2.6, 2.8, 3.0] ]
yticks_b = [ [-.01,0,.01], [-.05, 0, .05], [-0.05, 0, .05] ]
elif conf.subject == 2:
yticks_a = [ [.95, 1.0, 1.05], [-.1, -.05, 0, .05, .1], [2.6, 2.8, 3.0] ]
yticks_b = [ [-.01,0,.01], [-.05, 0, .05, 0.10], [-0.05, 0, .05] ]
ylims_a = [(.90, 1.16), (-.14, .34), (2.4, 3.5)]
ylims_b = [(-0.07, 0.0175),(-.295, .1475) , (-.35, .085)]
elif conf.subject == 3:
yticks_a = [ [.90, 0.95, 1.0], [-.1, -.05, 0, .05, .1], [2.9, 3.1, 3.3] ]
yticks_b = [ [-.01,0,.01], [-.05, 0, .05], [-0.05, 0, .05] ]
ylims_a = [(.87, 1.13), (-.14, .34), (2.8, 3.9)]
ylims_b = [(-0.07, 0.0175),(-.3, .0975) , (-.35, .085)]
else:
yticks_a = [ [.85, .9, .95], [-.1, -.05, 0, .05, .1], [2.6, 2.8, 3.0] ]
yticks_b = [ [-.01,0,.01], [-.05, 0, .05], [-0.05, 0, .05] ]
ylims_a = [(.82, 1.08), (-.14, .34), (2.5, 3.6)]
ylims_b = [(-0.07, 0.0175),(-.3, .0975) , (-.35, .085)]
T_ref = ws10.p.sim_t_r_per[-1]
xticks_a=arange(6) * .5 # adapt to xlim!
for dim in range(3):
axes_a[dim].set_xlim((0, 2.9))
axes_b[dim].set_xlim((0, 2.9))
axes_c1[dim].set_xlim((0, T_ref))
axes_c2[dim].set_xlim((0, T_ref))
axes_c2[dim].set_xticks([])
axes_c1[dim].set_xticks([])
axes_a[dim].set_xticks(xticks_a)
axes_a[0].set_ylabel('CoM height [m]', fontproperties=fp)
axes_a[1].set_ylabel('CoM lat. vel. [ms$^{-1}$]', fontproperties=fp)
axes_a[2].set_ylabel('CoM hor. vel. [ms$^{-1}$]', fontproperties=fp)
for ax in axes_a:
ax.get_yaxis().set_label_coords(-0.08, 0.5)
axes_c2[0].set_ylabel('dev. from ref. [m]', fontproperties=fp)
axes_c2[1].set_ylabel('dev. from ref. [ms$^{-1}$]', fontproperties=fp)
axes_c2[2].set_ylabel('dev. from ref. [ms$^{-1}$]', fontproperties=fp)
for ax in axes_c2:
ax.get_yaxis().set_label_coords(1.4,0.5)
axes_a[0].set_xticklabels([])
axes_a[1].set_xticklabels([])
axes_a[2].set_xticklabels(xticks_a, fontproperties=fp)
axes_a[2].set_xlabel('', fontproperties=fp)
# set axes limits and ticks
for ax, ylimit in zip(axes_a + axes_b, ylims_a + ylims_b):
ax.set_ylim(ylimit)
for ticks, ax in zip(yticks_a, axes_a):
ax.set_yticks(ticks)
ax.set_yticklabels(ticks, fontproperties = fp)
for ticks, ax in zip(yticks_b, axes_b):
ax.set_yticks(ticks)
ax.set_yticklabels(ticks, fontproperties = fp)
for ax in axes_c1:
ax.set_xticks([0, T_ref/2, T_ref])
ax.set_xticklabels([])
ax.tick_params(labelbottom='off', bottom='off' ) # don't put tick labels at the top
for ticks, ax in zip(yticks_b, axes_c2):
ax.set_yticks(ticks)
ax.set_yticklabels(ticks, fontproperties = fp)
ax.yaxis.set_label_position("right")
ax.set_xticks([0, T_ref/2, T_ref])
axes_c2[2].set_xticklabels(['0', '{:.2f}'.format(ws10.p.sim_t_r_per[-1]/2),
'{:.2f}'.format(ws10.p.sim_t_r_per[-1])], fontproperties=fp)
# plot "breaks"
if False:
for ax in axes_a:
size = .04
sx = 3.85 / 4 # size of x-axis
sx2 = .5
yl = ax.get_ylim()
dy = yl[1] - yl[0]
xl = ax.get_xlim()
ax.plot([xl[1] - sx*sx2*size, xl[1] + sx*sx2*size],[yl[0] - size*dy, yl[0] + size*dy],
'k', lw=1, clip_on=False)
ax.plot([xl[1] - sx*sx2*size, xl[1] + sx*sx2*size],[yl[1] - size*dy, yl[1] + size*dy],
'k', lw=1, clip_on=False)
ax.set_ylim(yl)
ax.set_xlim(xl)
# for broken axes
if False:
for ax in axes_c1:
size = .04
sx = 1
sx2 = .5
yl = ax.get_ylim()
dy = yl[1] - yl[0]
xl = ax.get_xlim()
ax.plot([xl[0] - sx*sx2*size, xl[0] + sx*sx2*size],[yl[0] - size*dy, yl[0] + size*dy],
'k', lw=1, clip_on=False)
ax.plot([xl[0] - sx*sx2*size, xl[0] + sx*sx2*size],[yl[1] - size*dy, yl[1] + size*dy],
'k', lw=1, clip_on=False)
ax.set_ylim(yl)
ax.set_xlim(xl)
axes_a[0].set_title('experimental data and model predictions', fontproperties=fp)
axes_c1[0].set_title(r'ref. orbits $\pm$ std', fontproperties=fp)
#axes_c2[2].xaxis.set_label_position("bottom")
# manual axis label + arrow
axes_a[2].text(0.25, axes_a[2].get_ylim()[0] - 2.5 + 2.385, 'time [s]', fontproperties=fp, ha='center', va='bottom')
axes_a[2].arrow(0.1, axes_a[2].get_ylim()[0] - 2.5 + 2.36, 0.3, 0, clip_on=False ) # width=
# remove spines, ticks and tick labels
for ax in axes_a + axes_b:
#ax.spines['right'].set_visible(False)
ax.tick_params(labelright='off', right='off' ) # don't put tick labels at the top
for ax in axes_c1 + axes_c2:
#ax.spines['left'].set_visible(False)
ax.tick_params(labelleft='off', left='off' ) # don't put tick labels at the top
for ax in axes_c1:
ax.tick_params(labelright='off')
for ax in axes_a + axes_b + axes_c1 + axes_c2:
ax.grid(True, color='#6f6f6f', linestyle='-', linewidth=.1)
# subplot labels
for ax, lbl in zip(axes_b + axes_c2, ['A', 'B', 'C', 'D', 'E', 'F']):
xl = ax.get_xlim()
yl = ax.get_ylim()
x_ = xl[0] + .05
y_ = yl[0] + .95*(yl[1] - yl[0])
ax.text(x_, y_, lbl, fontproperties=fp, bbox=dict(facecolor='#ffffff', edgecolor='k', pad=4.0), va='top')
fig
fig.savefig('img/prediction_examples_s{}_b.pdf'.format(conf.subject))
print 'stored as img/prediction_examples_s{}_b.pdf'.format(conf.subject)
fig
Out[46]:
In [47]:
# this cell is just to avoid scrolling ...
In [3]:
import models.slip as sl
import models.sliputil as su
ds = ws1.dataset_full
In [4]:
statesR = mean(ws1.dataset_full.all_IC_r, axis=0)
statesL = mean(ws1.dataset_full.all_IC_l, axis=0)
TR = mean(vstack(ws1.dataset_full.TR))
TL = mean(vstack(ws1.dataset_full.TL))
yR = mean(vstack(ws1.dataset_full.yminR))
yL = mean(vstack(ws1.dataset_full.yminL))
meanPR = mean(ws1.dataset_full.all_param_r, axis=0)
meanPL = mean(ws1.dataset_full.all_param_l, axis=0)
ppR, ppL = su.getPeriodicOrbit2(statesR, TR, yR, statesL, TL, yL, m=1., startParams=meanPR[:4])
print "difference periodic params vs. mean params in units of std's:"
print (ppR - meanPR) / std(ws1.dataset_full.all_param_r, axis=0)
print (ppL - meanPL) / std(ws1.dataset_full.all_param_l, axis=0)
In [5]:
# convenience function
def p2d(pars):
"""
Creates a SLIP param dictionary from given parameter vector.
Assumes that the mass is "1" (i.e. normalized).
:args:
pars (1-by-5 array): list of parameters: k, alpha, l0, beta, dE
:returns:
d: a dictionary of corresponding entires that can be passed to SLIP functions.
"""
d = {}
d['k'] = pars[0]
d['alpha'] = pars[1]
d['L0'] = pars[2]
d['beta'] = pars[3]
d['dE'] = pars[4]
d['m'] = 1.
d['g'] = -9.81
return d
In [6]:
# compute trends; split data into "baseline" and "detrended"
pR_dt = mi.dt_movingavg(ds.all_param_r, conf.dt_window, conf.dt_medfilter)
pR_base = ds.all_param_r - pR_dt
pL_dt = mi.dt_movingavg(ds.all_param_l, conf.dt_window, conf.dt_medfilter)
pL_base = ds.all_param_l - pL_dt
statesR_dt = mi.dt_movingavg(ds.all_kin_r, conf.dt_window, conf.dt_medfilter)
statesR_base = ds.all_kin_r - statesR_dt
statesL_dt = mi.dt_movingavg(ds.all_kin_l, conf.dt_window, conf.dt_medfilter)
statesL_base = ds.all_kin_l - statesL_dt
In [7]:
# compute predicted parameters
map_r = dot(pR_dt.T, pinv(statesR_dt.T))
ppred_r = pR_base + dot(map_r, statesR_dt.T).T
map_l = dot(pL_dt.T, pinv(statesL_dt.T))
ppred_l = pL_base + dot(map_l, statesL_dt.T).T
In [8]:
# here: perform actual predictions
# check every step
ppredR = [] # predicted data by "periodic param SLIP"
ppredL = []
bpredR = [] # predicted data by "baseline param SLIP"
bpredL = []
cpredR = [] # predicted data by controlled SLIP
cpredL = []
expR = [] # empirical *output* data; select only steps that can be simulated
expL = []
nbad = 0
for step in range(statesR_dt.shape[0]):
if mod(step,100) == 99:
sys.stdout.write('.')
pass
try:
# check if "true" parameters lead to correct output:
fs = su.finalState(ds.all_IC_l[step, :], p2d(ds.all_param_l[step,:]))
if norm(fs - ds.all_IC_r[step+1,:]) > 1e-5: # mismatch params <-> step
nbad += 1
continue
# simulate with fixed params
fsRp = su.finalState(ds.all_IC_r[step, :], p2d(ppR))
fsLp = su.finalState(ds.all_IC_l[step, :], p2d(ppL))
# here: variant, take baseline params instead of mean params
fsRb = su.finalState(ds.all_IC_r[step, :], p2d(pR_base[step, :]))
fsLb = su.finalState(ds.all_IC_l[step, :], p2d(pL_base[step, :]))
# simulate with controlled params
fsRc = su.finalState(ds.all_IC_r[step, :], p2d(ppred_r[step,:]))
fsLc = su.finalState(ds.all_IC_l[step, :], p2d(ppred_l[step,:]))
# now that everything worked well in this iteration, append data to lists
ppredR.append(fsRp)
ppredL.append(fsLp)
bpredR.append(fsRb)
bpredL.append(fsLb)
cpredR.append(fsRc)
cpredL.append(fsLc)
expR.append(ds.all_IC_l[step, :]) # empirical *output* data; select only steps that can be simulated
expL.append(ds.all_IC_r[step+1, :])
except:
nbad += 1
print "failed steps:", nbad, " / ", step + 1
#print step
ppredR = vstack(ppredR)
ppredL = vstack(ppredL)
bpredR = vstack(bpredR)
bpredL = vstack(bpredL)
cpredR = vstack(cpredR)
cpredL = vstack(cpredL)
expR = vstack(expR)
expL = vstack(expL)
In [9]:
# compute relative remaining variances for periodic parameter and controlled parameter SLIPs
dt = lambda x: mi.dt_movingavg(x, conf.dt_window, conf.dt_medfilter)
rv_pr = var(expR - ppredR, axis=0) / var(dt(expR), axis=0)
rv_pl = var(expL - ppredL, axis=0) / var(dt(expL), axis=0)
rv_cr = var(expR - cpredR, axis=0) / var(dt(expR), axis=0)
rv_cl = var(expL - cpredL, axis=0) / var(dt(expL), axis=0)
rv_br = var(expR - bpredR, axis=0) / var(dt(expR), axis=0)
rv_bl = var(expL - bpredL, axis=0) / var(dt(expL), axis=0)
print "periodic pars, R->L: ", rv_pr
print "periodic pars, L->R: ", rv_pl
print "baseline pars, R->L: ", rv_br
print "baseline pars, L->R: ", rv_bl
print "controlled pars, R->L:", rv_cr
print "controlled pars, L->R:", rv_cl
print "------------------ periodic vs. controlled parameter policy < [height vx vz] mean > ------------"
print "ratio (r):", rv_pr / rv_cr, mean(rv_pr / rv_cr)
print "ratio (l):", rv_pl / rv_cl, mean(rv_pl / rv_cl)
print "------------------ baseline vs. controlled parameter policy < [height vx vz] mean > ------------"
print "ratio (r):", rv_br / rv_cr, mean(rv_br / rv_cr)
print "ratio (l):", rv_bl / rv_cl, mean(rv_bl / rv_cl)
In [14]:
# this cell is just to avoid scrolling
In [14]:
# format:
# each row contains the relative remaining variance of [apex height, horiz. velocity, lat. velocity]
# different rows correspond to different subjects
# labels:
# per_(r|l): periodic parameter (right step / left step)
# base_(r|l): baseline SLIP parameter
# con_(r|l): full state used to predict SLIP parameters
per_r = array([[ 5.09813305, 1.41687185, 3.95499803],
[ 4.07074868, 2.22061749, 3.03771793],
[ 2.72895675, 1.02643295, 2.14058073],
[ 3.51679098, 1.48213309, 3.09847236]
])
per_l = array([[ 4.65053305, 1.7278126, 4.57193642],
[ 3.03587997, 1.6827558, 2.71372267],
[ 2.70720256, 1.10611103, 2.8442407 ],
[ 3.11069173, 1.66099706, 3.11306694]
])
base_r = array([[ 4.94496572, 1.37483645, 3.51303833],
[ 3.88095509, 2.12822966, 2.91185464],
[ 2.66116072, 1.00456918, 2.03773821],
[ 3.02570484, 1.33792675, 2.84498768]
])
base_l = array([[ 4.47654553, 1.69361897, 4.09989944],
[ 2.89052004, 1.59246907, 2.58903128],
[ 2.58522995, 1.07077452, 2.71590523],
[ 2.94880038, 1.5882737, 2.8433951 ]
])
con_r = array([[ 0.60158161, 0.20118681, 0.11897679],
[ 0.62847812, 0.31138729, 0.10432374],
[ 0.57383522, 0.1876361, 0.09251556],
[ 0.73224024, 0.28384424, 0.09220652]
])
con_l = array([[ 0.58484995, 0.23541731, 0.12558658],
[ 0.56799528, 0.27528579, 0.08815046],
[ 0.5334831, 0.17998224, 0.12302521],
[ 0.71691115, 0.29426182, 0.07861213]
])
print "average performance increase"
print " <height> <horiz. vel.> <lat vel.>"
print mean(per_r, axis=0) / mean(con_r, axis=0)
print mean(per_l, axis=0) / mean(con_l, axis=0)
# output is:
# <height> <horiz. vel.> <lat vel.>
#[ 6.07799991 6.2456457 29.97816481]
#[ 5.61921 6.27208925 31.8820018 ]
print "\nminimal performance increase"
print " <height> <horiz. vel.> <lat vel.>"
print ["{:.4f}".format(min(per_r[:, col]/con_r[:, col])) for col in range(3)]
print ["{:.4f}".format(min(per_l[:, col]/con_l[:, col])) for col in range(3)]
# output is:
#minimal performance increase
# <height> <horiz. vel.> <lat vel.>
#['4.7556', '5.2216', '23.1375']
#['4.3390', '5.6446', '23.1192']
In [13]:
In [ ]:
# prelimi
In [ ]:
# compare the L2-norms of the difference SLIP - experiment to the L2-norm of the experiment.
#Here we show that SLIP can
#reproduce experimental observations of human run-
#ning dynamics (forces and velocities) to within (SR:
#±XX%)
In [ ]:
# approach:
## do not obtain forces. compute only for com positions and velocities
## phase left is later than phase right
# for each trial in ws1.k.raw_dat:
# take initial apex time TR (this is already quadratically interpolated)
# take corresponding apex time TL ()
# ws1.dataset_full.TR[0][:3]
# NOTE: check consistency of times
In [33]:
tar = ws1.k.get_kin_apex(ws1.dataset_full.all_phases_r, return_times=True)
tal = ws1.k.get_kin_apex(ws1.dataset_full.all_phases_l, return_times=True)
In [68]:
# required: absolute times of apices
# approach: compute apex closest to all_phases_r
rd = ws1.k.raw_dat[0]
In [88]:
vz = gradient(rd['com'][:,2])
phi = rd['phi2'].squeeze()
all_roots = []
all_T = []
for theta in ws1.dataset_full.all_phases_r[0]:
idx = argmin(abs(theta - phi))
# find zero crossing of vz[idx-3:idx+3]
if vz[idx-2] < 0 or vz[idx+2] > 0:
raise ValueError("idx {} is not an apex!".format(idx))
# interpolate around apex
p = polyfit(arange(-2,3),vz[idx-2: idx+3],1)
idx0 = roots(p)
all_roots.append(idx0)
all_T.append((idx + idx0) / 250.)
all_T = array(all_T)
In [91]:
t_vec = linspace(0,240,vz.shape[0], endpoint=False)
figure()
plot(t_vec, vz,'b')
plot(all_T, 0*all_T, 'rd')
xlim(0,1)
Out[91]:
In [83]:
figure()
plot(vz[115:125],'.-')
plot([5-0.785, 5-0.785,],[-0.0001,0.0001],'k')
plot([3,5],[0,0],'k')
Out[83]:
In [45]:
[(len(TR) - len(TL)) for TR, TL in zip (ws1.dataset_full.TR, ws1.dataset_full.TL)]
Out[45]:
In [ ]: